/**
 * 1.version:2.2 data:2023/5/20
 * 2.用于驱动HT03电机
 * 3.注意can线序问题
 * 4.电机逆时针转为正
 */

#ifndef HT03_DRIVER
#define HT03_DRIVER

#ifdef __cplusplus
extern "C"
{
#endif

#include "main.h"
#include "can.h"

#ifndef PI
#define PI 3.14159265358979f
#endif

    /* 角度转弧度 */
#define RAD(deg) ((deg) * PI / 180)
    /* 弧度转角度 */
#define DEG(rad) ((rad) * 180 / PI)

    /**
     * ***HT电机参数***
     *
     * Position: 位置 rad
     * Velocity: 速度 rad/s
     * Kp: 位置增益
     * Kd: 速度增益
     * T: 力矩 N*M
     */
    typedef struct
    {
        CAN_HandleTypeDef *can; // 电机CAN
        uint8_t id;             // 电机ID
        float torque;           // 力矩
        float velocity;         // 速度
        float position;         // 位置
        float kp;               // 刚度系数
        float kd;               // 阻尼系数

        float target_torque;   // 设置力矩
        float target_velocity; // 设置速度
        float target_position; // 设置位置
        float target_kp;       // 设置Kp
        float target_kd;       // 设置Kd
    } HT_t;

    void HT_can_callback(CAN_RxHeaderTypeDef *pheader, uint8_t *pbuf);
    void HT_init(HT_t *motor, uint8_t id, CAN_HandleTypeDef *can);

    void HT_run(HT_t *motor);
    void HT_set_position(HT_t *motor, float angle, float kp, float kd, float torque);
    void HT_set_speed(HT_t *motor, float speed, float kd);
    void HT_set_torque(HT_t *motor, float torque);
    void HT_set_damp(HT_t *motor, float Kd);
    void HT_set_zero_torque(HT_t *motor);
    void HT_send_parameter(HT_t *motor, float torque, float speed, float angle, float kp, float kd);
    void HT_set_zero_position(HT_t *motor);
    uint8_t HT_arrival_position(HT_t *motor, float per);

#ifdef __cplusplus
}
#endif

#endif
